/* -*- Mode: C; indent-tabs-mode: t; c-basic-offset: 4; tab-width: 4 -*- */
/*
 * assign4
 * Copyright (C)  Michael 2008 <mikey.morckos@gmail.com>
 * 
 * assign4 is free software: you can redistribute it and/or modify it
 * under the terms of the GNU General Public License as published by the
 * Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 * 
 * assign4 is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
 * See the GNU General Public License for more details.
 * 
 * You should have received a copy of the GNU General Public License along
 * with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

#include "Q4.h"

/* INSTRUCTOR */
Q4 :: Q4 () 

: button_1_1("Go!"),

  label1("Width :"),
  label2("Height :"),
  label3("Alpha u :"),
  label4("Alpha v :"),
  label5("Tx :"),
  label6("Ty :"),
  label7("Tz :"),
  label8("Pan :"),
  label9("Tilt :"),
  label10("Swing :"),

  frame2("Canny"),
  main_hbox(false, 5)

{
    set_border_width(10);
    set_title("Image Editor");
	
    add(main_hbox);
	
    main_hbox.set_border_width(20);
    main_hbox.pack_start(main_vbox_1);
    
    main_vbox_1.pack_start(frame2, PACK_EXPAND_WIDGET, 20);
	
    /* Frame */
    frame2.add(frame2_vbox);
	
    frame2_vbox.pack_start(label1);
    frame2_vbox.pack_start(entry1, PACK_EXPAND_WIDGET, 5);
	
    frame2_vbox.pack_start(label2);
    frame2_vbox.pack_start(entry2, PACK_EXPAND_WIDGET, 5);
    
    frame2_vbox.pack_start(label3);
    frame2_vbox.pack_start(entry3, PACK_EXPAND_WIDGET, 5);
    
    frame2_vbox.pack_start(label4);
    frame2_vbox.pack_start(entry4, PACK_EXPAND_WIDGET, 5);
    
    frame2_vbox.pack_start(label5);
    frame2_vbox.pack_start(entry5, PACK_EXPAND_WIDGET, 5);
    
    frame2_vbox.pack_start(label6);
    frame2_vbox.pack_start(entry6, PACK_EXPAND_WIDGET, 5);
    
    frame2_vbox.pack_start(label7);
    frame2_vbox.pack_start(entry7, PACK_EXPAND_WIDGET, 5);
    
    frame2_vbox.pack_start(label8);
    frame2_vbox.pack_start(entry8, PACK_EXPAND_WIDGET, 5);
    
    frame2_vbox.pack_start(label9);
    frame2_vbox.pack_start(entry9, PACK_EXPAND_WIDGET, 5);
    
    frame2_vbox.pack_start(label10);
    frame2_vbox.pack_start(entry10, PACK_EXPAND_WIDGET, 5);

    frame2_vbox.pack_start(button_1_1, PACK_EXPAND_WIDGET, 5);
    button_1_1.signal_clicked().connect(sigc :: mem_fun(*this, &Q4 :: get_Homo));

    show_all_children();
    
    /* INITS */
    
    name1 = "Image-1";
    name2 = "Image-2";
	
	img1 = 0;
	img2 = 0;
	
	img1 = cvLoadImage("cedfence.bmp", 1);
	
	img_height = img1->height;
	img_width = img1->width;
	
	img_data.set_IMG(img1);
	
	cvNamedWindow(name1, 3);
	cvNamedWindow(name2, 3);
	
	cvShowImage(name1, img1);
}

/* DESTRUCTOR */
Q4 :: ~Q4 () {
	
	cout << "Closing..." << endl;
	
	cvReleaseImage(&img1);
	cvReleaseImage(&img2);

	cvDestroyWindow(name1);
	cvDestroyWindow(name2);
}

void Q4 :: get_Homo() {
	
	/* USER INPUT */
	res_width = atoi((entry1.get_text()).c_str());
	res_height = atoi((entry2.get_text()).c_str());
	alpha_u = atoi((entry3.get_text()).c_str());
	alpha_v = atoi((entry4.get_text()).c_str());
	
	tx = atoi((entry5.get_text()).c_str());
	ty = atoi((entry6.get_text()).c_str());
	tz = atoi((entry7.get_text()).c_str());
	
	pan = atoi((entry8.get_text()).c_str()) * PI / 180.0;
	tilt = atoi((entry9.get_text()).c_str()) * PI / 180.0;
	swing = atoi((entry10.get_text()).c_str()) * PI / 180.0;
	
	img2 = cvCreateImage(cvSize(res_width, res_height), img1->depth, img1->nChannels);
	
	/* R MATRIX */
	Pan_arr[0][0] = (double) cos(pan);
	Pan_arr[0][1] = (double) sin(pan);
	Pan_arr[0][2] = 0.0;
	Pan_arr[1][0] = (double) -sin(pan);
	Pan_arr[1][1] = (double) cos(pan);
	Pan_arr[1][2] = 0.0;
	Pan_arr[2][0] = 0.0;
	Pan_arr[2][1] = 0.0;
	Pan_arr[2][2] = 1.0;
	
	Tilt_arr[0][0] = 1.0;
	Tilt_arr[0][1] = 0.0;
	Tilt_arr[0][2] = 0.0;
	Tilt_arr[1][0] = 0.0;
	Tilt_arr[2][0] = 0.0;
	Tilt_arr[1][1] = (double) cos(tilt);
	Tilt_arr[1][2] = (double) sin(tilt);
	Tilt_arr[2][1] = (double) -sin(tilt);
	Tilt_arr[2][2] = (double) cos(tilt);
	
	Swing_arr[0][0] = (double) cos(swing);
	Swing_arr[0][1] = (double) sin(swing);
	Swing_arr[0][2] = 0.0;
	Swing_arr[1][0] = (double) -sin(swing);
	Swing_arr[1][1] = (double) cos(swing);
	Swing_arr[1][2] = 0.0;
	Swing_arr[2][0] = 0.0;
	Swing_arr[2][1] = 0.0;
	Swing_arr[2][2] = 1.0;
	
	tmp_R = cvMat(3, 3, CV_64FC1, R_arr);
	R = &tmp_R;
	
	tmp_Pan = cvMat(3, 3, CV_64FC1, Pan_arr);
	Pan = &tmp_Pan;
	
	tmp_Tilt = cvMat(3, 3, CV_64FC1, Tilt_arr);
	Tilt = &tmp_Tilt;
	
	tmp_Swing = cvMat(3, 3, CV_64FC1, Swing_arr);
	Swing = &tmp_Swing;
	
	tmp_TMP = cvMat(3, 3, CV_64FC1, TMP_arr);
	TMP = &tmp_TMP;
	
	cvMatMul(Tilt, Pan, TMP);
	cvMatMul(Swing, TMP, R);
	
	/* A MATRIX */
	A_arr[0][0] = alpha_u;
	A_arr[1][1] = alpha_v;
	A_arr[0][1] = 0.0;
	A_arr[1][0] = 0.0;
	A_arr[2][0] = 0.0;
	A_arr[2][1] = 0.0;
	A_arr[0][2] = res_width / 2;
	A_arr[1][2] = res_height / 2 ;
	A_arr[2][2] = 1.0;
	
	tmp_A = cvMat(3, 3, CV_64FC1, A_arr);
	A = &tmp_A;
	
	/* T MATRIX */
	T_arr[0][0] = tx;
	T_arr[1][0] = ty;
	T_arr[2][0] = tz;
	
	tmp_T = cvMat(3, 1, CV_64FC1, T_arr);
	T = &tmp_T;
	
	/* HOMOGRAPHY CALCULATIONS */
	tmp_TMP2 = cvMat(3, 1, CV_64FC1, TMP2_arr);
	TMP2 = &tmp_TMP2;
	
	tmp_Homo = cvMat(3, 3, CV_64FC1, Homo_arr);
	Homo = &tmp_Homo;
	
	tmp_Homo_invert = cvMat(3, 3, CV_64FC1, Homo_arr_invert);
	Homo_invert = &tmp_Homo_invert;
	
	cvMatMul(A, R, Homo);
	cvMatMul(Homo, T, TMP2);
	
	Homo_arr[0][2] = -cvmGet(TMP2, 0, 0);
	Homo_arr[1][2] = -cvmGet(TMP2, 1, 0);
	Homo_arr[2][2] = -cvmGet(TMP2, 2, 0);
	
	cvInvert(Homo, Homo_invert);
	
	for(int i = 0; i < 3; i++) {
		for(int j = 0; j < 3; j++)
			cout << cvmGet(Homo, i, j) << " ";
	
		cout << endl;
	}
	
	apply_Homo();
}

void Q4 :: apply_Homo() {
	
	CvScalar s;
	double point_arr[3][1];
	CvMat p_tmp;
	CvMat* point; 
	
	double result_arr[3][1];
	CvMat result;
	CvMat* result_point;
	
	int x, y;
	
	for(int i = 0; i < res_height; i++) {
		for(int j = 0; j < res_width; j++) {
			point_arr[0][0] = (double) i;
			point_arr[1][0] = (double) j;
	        point_arr[2][0] = 1.0;
		
			p_tmp = cvMat(3, 1, CV_64FC1, point_arr);
			point = &p_tmp;
			
			result = cvMat(3, 1, CV_64FC1, result_arr);
			result_point = &result;
			
			cvMatMul(Homo_invert, point, result_point);
			
			/* NORMALIZE */
			x = round(cvmGet(result_point, 0, 0) / cvmGet(result_point, 2, 0));
			y = round(cvmGet(result_point, 1, 0) / cvmGet(result_point, 2, 0));
			
			s = cvGet2D(img2, i, j);
			/* IF THE RESULTING COORDINATES IS INTO RANGE
			   COPY THE CORRESPONDING INTENSITY IN IMG1 TO THE CURRENT
			   POSITION IN IMG2 
			*/
			if(x >= 0 && x < img_width && y >= 0 && x < img_height) {
				s.val[0] = (int) img_data[x][y].b;
		        s.val[1] = (int) img_data[x][y].g;
		        s.val[2] = (int) img_data[x][y].r;
			}
			else {
				s.val[0] = 0;
		        s.val[1] = 0;
		        s.val[2] = 0;
			}	
			cvSet2D(img2, i, j, s);
		}
	}

	cvShowImage(name2, img2);	
}	

/* MAIN */
int main (int argc, char *argv[]) {

    Gtk::Main kit(argc, argv);	

    Q4 editor;
    Gtk::Main::run(editor);

    return 0;
}

